/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.PID;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;

/**
 *
 * @author Jared
 */
//This is an example of a very simple computer vision system
//It locates the biggest red object the camera can see and 
//prints the target location
//I've never tested it, so it might not work
//If we do vision, we may do it on the robot like this example
//Or we may do it on the driver station 
//If we did it on the driver station, we'd probably write it in labview
public class VisionProcessing implements Runnable {//Implements runable means that it is threaded 
    //and will run as a seperate task on the robot processor
    
        ColorImage camImage;
        BinaryImage thresholdImage;
        BinaryImage filteredImage; //Will remove tiny particles
        CriteriaCollection cc;
        ParticleAnalysisReport[] reports;
        ParticleAnalysisReport mainTarget;
        Timer visionTimer;
    public VisionProcessing(){
        new Thread(this).start();
        AxisCamera.getInstance();
        AxisCamera.getInstance().writeBrightness(50);
        visionTimer = new Timer();
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, 10, 9999, false);//It this was actually 
        //used, I'd have WAY more filter criteria
        //Creates a new Thread object with a target of this class and runs the start
        //method.  More simply, this just starts what is in the run method of this class
        
        //Set up camera.  There's other settings I could play with
    }
    public void run() {
        while(true){
            try {
                visionTimer.reset();
                visionTimer.start();
                camImage = AxisCamera.getInstance().getImage();
                thresholdImage = camImage.thresholdRGB(200, 255, 0, 20, 0, 20);
                filteredImage = thresholdImage.particleFilter(cc);//Removes stuff that doesn't meet cc criteria
                reports = filteredImage.getOrderedParticleAnalysisReports(1);//gets only biggest particle
                mainTarget = reports[0]; //gets the BIGGEST particle
                visionTimer.stop();
                camImage.free();
                thresholdImage.free();
                filteredImage.free();
                System.out.println("Took "+visionTimer.get());
                System.out.println("Target is at "+mainTarget.center_mass_x_normalized+ ", "+mainTarget.center_mass_y_normalized);
                //This output stuff slows down the image framerate, so I've added a timer
                //which can tells us how long it takes for one image to be processed
            } catch (AxisCameraException ex) {
                ex.printStackTrace();
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            }
            
            
            
            try {
                Thread.sleep(100);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
    }
    
}
